/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * command.h
 *
 * PURPOSE:
 *  This code implements the command data structure between tasks on
 *  the flight computer and from ground control station
 *
 * CREATED:
 * 11/2000 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/07/26 20:34:24 $
 * $Revision: 1.5 $
 *
 ***********************************************************************/

#ifndef _COMMAND_H
#define _COMMAND_H

#include "commandStruct.h"
#include "definedCommands.h"
#include "type_defs.h"

#include <iostream>
#include <vector>
#include <linux/limits.h>

using namespace std;

// Define a template class for a vector of IDs.
typedef vector<uint8_t> CMDVECTOR;

//-- Define the type of commands --//
typedef enum _CommandType_t
{
	CMD_NO_TYPE=0,
	CMD_STREAM_REQUEST,
	CMD_DATA_STREAM,
	CMD_COMMAND,
	CMD_STREAM_COMMAND,
	CMD_ACK_NACK,
} CommandType_t;

/***********************************************************************
 * This struct is an array of commands that is used to
 * be the defined commands in the system. The maximum length
 * of the array is set by CMD_MAX_NUM
 *
 * id number can never be 255 - implies non-defined command
 * numParams              65536 - implies dynamic size
 ***********************************************************************/
typedef struct _DefinedCommandStruct
{
	unsigned char       id;
	uint16_t            numParams;
	const char *        name;
	CommandType_t       type;
} DefinedCommandStruct;
#define CMD_DYNAMIC_SIZE 65535
//
// Maximum number of parameters is set to size of an atomic write with a pipe, set as 
// PIPE_BUF. In Linux, this is 4096. Techically we can use upto 65535 as the maximum command
// size.
//
#define CMD_HEADER_SIZE 4	// cmd, seq, num bytes
#define CMD_CSUM_SIZE   2	// checksum size
#define CMD_PACKET_SIZE 6 	// min size of a packet (HEADER + CSUM)
#define CMD_MAX_SIZE    PIPE_BUF // max size of command 
#define CMD_MAX_PARAM   (CMD_MAX_SIZE-CMD_PACKET_SIZE) // num of avail params 
#define CMD_MAX_NUM	   256	// maximum number of commands

//
// Extern the defined commands which are in command.cxx
extern const DefinedCommandStruct DefinedCommands[CMD_MAX_NUM];

//
// Use these functions very carefully
// They are only ment for uint8_t buffer uses, and the CSUM
// is not vector compatible
//
#define CMD_CMD(buf)       ( buf[0] )
#define CMD_SEQ(buf)       ( buf[1] )
#define CMD_NUM(buf)       ( *((uint16_t *)(&buf[2])) )
#define CMD_PARAM(buf,num) ( buf[CMD_HEADER_SIZE+num] )
#define CMD_DATAPTR(buf)   ( (uint8_t *)(buf+CMD_HEADER_SIZE) )
#define CMD_CSUM(buf)      (*((uint16_t *)(buf+CMD_NUM(buf)+CMD_HEADER_SIZE)))
#define CMD_SIZE(buf)      ( CMD_NUM(buf) + CMD_PACKET_SIZE )

//
// <summary> defines the command structure </summary>
//
// This code implements the command data structure between tasks on the flight 
// computer and from ground control station.  This structure of a command 
// is as follows:
// <pre>
//	    Description    |    Name    |   Data Type
//	    --------------------------------------------------
//	    Command	       | Cmd      |   unsigned byte 
//	    sequence #     | Seq      |   unsigned byte 
//	    count of param | Num      |   unsigned 16-bit int 
//	    Parameters     | Param[]  |   array of unsigned bytes
//	    Checksum       | Csum     |   unsigned 16-bit int 
// </pre>
//
// <p><b> Cmd </b><p>
// ------------------------------------------------------------<p><b>
// This field represents which command and or data set is contained within
// the message.  For commands that have the same meaning across different
// systems, the same cmd number should be used.
//
// <p><b> Num </b><p>
// ------------------------------------------------------------<p><b>
// The number of bytes contained within the actual parameters, which are
// held in the param[] field.  
//
// <p><b> Param[] </b><p>
// ------------------------------------------------------------<p><b>
// This is an array of unsigned bytes that are used to carry the data
// of the command or data set.  If the contents of the data represent
// data types that require more than one byte ( short, int, long, float,
// double ) then the byte ordering should be that of the host computer.  Thus
// for a x86 flight computer, the byte ordering will be little-endian. 
//
// <p><b> Csum </b><p>
// ------------------------------------------------------------<p><b>
// The checksum field utilizes the TCP checksum calculation, which is briefly 
// described as ( see Network Working Group RFC 1071) :
//
//	Sender: compute the ones-complement sum of the data, viewed as a
//		sequence of two-byte words. (The ones-complement sum is the
//		sum with carries out of the most-significant bit wrapped around
//		into the least-significant bit.) If the length of the data in
//		bytes is odd, logically add one zero byte to the data for the
//		computation. Take the bitwise-complement of the resulting sum,
//		and place it in the checksum field. (The checksum field is either
//		not included or is set to zero in the original computation.)
//  
//	Receiver: compute the ones complement sum of the data, viewed as a
//		sequence of two-byte words. If it is 0x0000, accept the data,
//		otherwise discard it as corrupted.
//
// The TCP checksum is a 16-bit ones complement sum of the data. This sum 
// will catch any burst error of 16 bits or less, and over uniformly 
// distributed values of data is expected to detect other types of errors 
// at a rate proportional to 1 in 2^16.  A limitation to the TCP checksum
// is that it can not detect an even number of bit flips on the same bit
// pocession of the data, but if the bit posession of the flip is different
// it will detect it.  In general, byte ordering is not a problem, ie 
// little-endian or big-endian, and this code can be used on any *endian
// machine. The problem will arise if the flight computer is say a 
// motorola powerPC which uses big-endian is communicating to an Intel ground 
// station which is little-endian.  This problem should be dealt with by the 
// communication interface on the ground computer though, and not by this
// task.
//
// <p><b> Changes/Improvements </b><p>
// ------------------------------------------------------------<p><b>
// The TO & CMD fields provide 2^16 different commands total, or 256 commands
// per system, which is a little overboard.  These two fields could probably
// be compressed into a single byte field, where say the first 3 bits represent
// the system and the last 5 give the command, which would be more effienct use
// of space.  
//
// The length field could be removed as well, and replaced with a static look-up
// table, thus reducing the command length by another byte.  The trade off will
// be that two memory access will be required to get the length of the data
// in the PARAM[] field.  This lookup table should obviously be based on the 
// TO & CMD fields ( or the combined one mentioned above ).
//
// Another checksum alogrithm which may be of interest is the 8-bit Fletcher
// algorithm.  This algorithm has a higher error detection rate, but requires
// two additions for each 8-bit data and is dependent upon data ordering,
//

uint16_t C_calcCheckSum(uint8_t* command = NULL);

class Command {

	private:
		uint8_t *command;
		uint16_t length;
		uint8_t  seqNo;

		// private functions

	public:

		// constructors
		// don't assume that the length of the vector
		// is the same as the command, so get the length
		// from whats in the vector
		Command(int size=CMD_PACKET_SIZE);
		Command(const Command &);
		Command(const uint8_t * const buf);  
		Command(CMDVECTOR &buf);  

		// destructor
		~Command();

		// data access functions <p>
		// Each of the functions provide a get and set 
		// functionality.  By providing a argument to the
		// function, you are setting that value.<p>
		// For getting/setting the parameter values of the
		// command, you must always specify an index of
		// the paramter, 0 being the first element.<p>
		// The check-sum has a different syntax since it
		// is unlike the other values which are unsigned char
		// types, it is an unsigned short int, and needs
		// to be handled as one.
		uint8_t 	cmd() const;
		void 	cmd(uint8_t cmdId);

		uint8_t 	seq() const;
		void 	seq(uint8_t seqNo);

		uint16_t 	num() const;
		void 	num(uint16_t numId);

		uint8_t 	param(uint8_t num) const;
		uint8_t 	param(uint8_t num, uint8_t p);
		void 	getParams(uint8_t * const buf) const;
		void 	setParams(const uint8_t * const buf, int length);

		uint16_t 	csum() const;
		void 	setCheckSum();

		// returns the total number of bytes in the command,
		// including header and csum bytes.
		int 		size() const;

		// see if the csum in the command is equal to what we calculate
		// for the bytes in the command
		bool isValid() const;
		uint16_t calcCheckSum() const;

		// see if the command is a known and defined command
		bool isDefined() const;
		CommandType_t getType() const;
		const char *const getName() const;

		// buffer operations, sizes of buffer must be guaranteed by caller.
		// By fill a command from a buffer, the csum is the same as in the 
		// buffer, thus if you are creating a command you need to set the
		// check-sum value after calling the copyFromBuf function.
		void copyFromBuf(const uint8_t * const buf ); 
		void copyToBuf(const uint8_t * buf) const;   

		// debuging output.  The printCmd is the same as using the
		// ostream friend function like shown
		// 	<src> cout << printCmd() </src>
		friend ostream& operator << (ostream& ostrm, const Command& cmd) ;
		void printCmd() const;

		// Assignment operators
		Command& operator = (const Command& cmd);
		Command& operator = (const CMDVECTOR& cmd);
		Command& operator = (const uint8_t *buf);

		// boolean equality operators
		bool operator == (const Command& cmd) const;
		bool operator != (const Command& cmd) const;
};

// <sumary> 
// A class to decompose commands out of an error prone 
// stream of bytes 
// </summary>
// The purpose of this class is to parse out commands from 
// a data stream which is error prone and asynchronous.  That
// is, an actual package may be split between multiple reads
// from the port.
class CommandQueue
{

	public:
		CommandQueue() { ; }
		~CommandQueue() {;}

		// append to the queue, a buffer of length num
		void push(uint8_t * buf, size_t num) ;

		// pop a single byte off of the queue
		uint8_t popByte() ;
		void pop() ;

		// pop a buffer of maximum length bytes off of the queue
		// returns the number actual popped off
		int popBuf(char *buf, size_t length) ;

		// pop a command and move down the received bytes by the
		// size of the command
		Command popCommand() ;
		Command peekCommand() ;

		// clears the contents
		void clear() {
			return fifo.clear();
		}

		// checks to see if the next set of bytes on the byte
		// queue form a valid command
		bool isValid() const ;
		// checks to see if the next set of bytes on the byte
		// queue form a valid command
		bool isDefined() const ;

		// check to see if there are enough bytes to make 
		// the command complete
		bool needMoreBytes() const;

		// get the number of bytes in the queue
		size_t length() const {
			return fifo.size();
		}
		size_t size() const {
			return fifo.size();
		}

	private:

		CMDVECTOR fifo;

		// private functions
		uint16_t calcCheckSum() const;
};

#endif // _COMMAND_H
